using System;
using Geometry;

namespace PotentialFieldDriver
{
    /// <summary>
    /// wall
    /// </summary>
    public class Wall2D
    {
        public Point2D firstPoint;
        public Point2D secondPoint;

        private Line2D line;
        private Rectangle2D rectangle;

        public Wall2D(Point2D firstPoint, Point2D secondPoint)
        {
            this.firstPoint = firstPoint;
            this.secondPoint = secondPoint;

            this.line = new Line2D(firstPoint.x, firstPoint.y, secondPoint.x, secondPoint.y);
            this.rectangle = new Rectangle2D(Math.Min(firstPoint.x, secondPoint.x), Math.Min(firstPoint.y, secondPoint.y),
                Math.Abs(firstPoint.x - secondPoint.x), Math.Abs(firstPoint.y - secondPoint.y));
        }

        public unsafe PotencialDriver.AngleLock GetLockForRobot(double robotX, double robotY, double lookupDistance)
        {
            Circle2D robotLookupCircle = new Circle2D(robotX, robotY, lookupDistance);
            Point2D pointOnLine1, pointOnLine2;
            int nrOfPoints = robotLookupCircle.GetIntersectionPoints(line, &pointOnLine1.x, &pointOnLine1.y, &pointOnLine2.x, &pointOnLine2.y);
            if (nrOfPoints < 2)
                return null;

            Vector2D firstVector;
            Vector2D secondVector;

            if (!rectangle.Contains(pointOnLine1) && !rectangle.Contains(pointOnLine2))
            {
                // must check if all section is in lookup range
                if (firstPoint.GetDistance(robotX, robotY) < lookupDistance) // it is
                {
                    firstVector = new Vector2D(firstPoint.x - robotX, firstPoint.y - robotY);
                    secondVector = new Vector2D(secondPoint.x - robotX, secondPoint.y - robotY);
                }
                else
                {
                    return null;
                }
            }
            else if (rectangle.Contains(pointOnLine1) && rectangle.Contains(pointOnLine2))
            {
                firstVector = new Vector2D(pointOnLine1.x - robotX, pointOnLine1.y - robotY);
                secondVector = new Vector2D(pointOnLine2.x - robotX, pointOnLine2.y - robotY);
            }
            else // exacly one intersecting point is within the section
            {
                firstVector = new Vector2D(firstPoint.x - robotX, firstPoint.y - robotY);
                secondVector = new Vector2D(secondPoint.x - robotX, secondPoint.y - robotY);
                if (secondVector.Length < firstVector.Length)
                    firstVector = secondVector;
                if (rectangle.Contains(pointOnLine1))
                    secondVector = new Vector2D(pointOnLine1.x - robotX, pointOnLine1.y - robotY);
                else
                    secondVector = new Vector2D(pointOnLine2.x - robotX, pointOnLine2.y - robotY);
            }

            double angleBetween = Vector2D.AngleBetween(firstVector, secondVector);
            double addedAngle = Math.Max(
                Math.Atan(PotencialDriver.wheelsHalfDistance / firstVector.Length),
                Math.Atan(PotencialDriver.wheelsHalfDistance / secondVector.Length));
            return new PotencialDriver.AngleLock(firstVector.Angle + angleBetween / 2,
                Math.Abs(angleBetween / 2) + 2 * addedAngle,
                Math.Min(firstVector.Length, secondVector.Length));

        }

        public double getMinX()
        {
            return firstPoint.x < secondPoint.x ? firstPoint.x : secondPoint.x;
        }

        public double getMaxX()
        {
            return firstPoint.x > secondPoint.x ? firstPoint.x : secondPoint.x;
        }

        public double getMinY()
        {
            return firstPoint.y < secondPoint.y ? firstPoint.y : secondPoint.y;
        }

        public double getMaxY()
        {
            return firstPoint.y > secondPoint.y ? firstPoint.y : secondPoint.y;
        }

        public override string ToString()
        {
            return "Wall: (" + firstPoint.x + ", " + firstPoint.y + ") - (" + secondPoint.x + ", " + secondPoint.y + ")";
        }

    }

}
